from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from nav_msgs.msg import Odometry


class TransformUGV(Node):
    def __init__(self):
        super().__init__('ugv_tf_node')

        self.tf_broadcaster = TransformBroadcaster(self)

        self.odom_sub = self.create_subscription(Odometry,"/out/odom",self.odom_callback,10)

    def odom_callback(self,msg:Odometry):
        stamp = msg.header.stamp
        frame_id = msg.header.frame_id
        child_frame_id = msg.child_frame_id

        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        z = msg.pose.pose.position.z

        q_w = msg.pose.pose.orientation.w
        q_x = msg.pose.pose.orientation.x
        q_y = msg.pose.pose.orientation.y
        q_z = msg.pose.pose.orientation.z

        odom2base_footprint = TransformStamped()
        odom2base_footprint.header.stamp = stamp
        odom2base_footprint.header.frame_id = frame_id
        odom2base_footprint.child_frame_id = child_frame_id
        odom2base_footprint.transform.translation.x = x
        odom2base_footprint.transform.translation.y = y
        odom2base_footprint.transform.translation.z = z
        odom2base_footprint.transform.rotation.w = q_w
        odom2base_footprint.transform.rotation.x = q_x
        odom2base_footprint.transform.rotation.y = q_y
        odom2base_footprint.transform.rotation.z = q_z

        self.tf_broadcaster.sendTransform(odom2base_footprint)

def main(args=None):
    try:
        rclpy.init(args=args)
        node = TransformUGV()
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass